#include "sensor_autobase.h"
#include "autobase_initialize.h"

static int32 m_n;
static float m_b0;
static float m_b;
static float m_sg;
static float m_diff;

void AB_Init()
{
    autobase_initialize();
}

void AB_Start(uint8 calib, uint32 n, float b0, float b)
{
    if (rtIsNaN(b0) && n>432)
        n = 432;
    m_n = n;
    m_diff = rtNaN;
    autobase(calib ? -1 : 0, rtNaN, b0, b, 0, rtNaN, &m_sg, &m_b0, &m_b);
}

float AB_Calc(float sg, uint8 badsg, float minsg, uint8 update)
{

    if (update)
    {
        m_n++;
        autobase(m_n, sg, m_b0, m_b, badsg, minsg, &m_sg, &m_b0, &m_b);
        m_diff = m_sg - sg;
    }
    else if (!rtIsNaN(m_diff))
    {
        m_sg = sg + m_diff;
    }
//    else if (!rtIsNaN(m_b0) && !rtIsNaN(m_b))
//    {
//        m_sg = sg + m_b0 - m_b;
//    }
    else
    {
        m_sg = sg;
    }
    return m_sg;
}

void AB_Calib(float sg, float b)
{
//    m_n = 0;
    m_diff = 0;
    autobase(-2, sg, b, rtNaN, 1, rtNaN, &m_sg, &m_b0, &m_b);
}

int32 AB_GetN()
{
    return m_n;
}

float AB_GetB0()
{
    return m_b0;
}

float AB_GetB()
{
    return m_b;
}
